#include <Eigen/Dense>
#include <iostream>

int main()
{
    Eigen::Vector2f p_cam_left;
    p_cam_left<<-596.41016151,233.01270189;
    Eigen::Vector2f p_cam_right;
    p_cam_right<<-163.39745962,483.01270189;
    Eigen::Vector2f LEFT_COLUMN_COORDS;
    LEFT_COLUMN_COORDS<<0,0;
    Eigen::Vector2f RIGHT_COLUMN_COORDS;
    RIGHT_COLUMN_COORDS<<500,0;
    Eigen::Vector2f p_cam_c = (p_cam_left + p_cam_right)/2.0f;
    Eigen::Vector2f p_world_c = (LEFT_COLUMN_COORDS + RIGHT_COLUMN_COORDS)/2.0f;

    Eigen::Matrix2f S = Eigen::Matrix2f::Zero();
    S += (p_cam_left - p_cam_c)*(LEFT_COLUMN_COORDS - p_world_c).transpose();
    S += (p_cam_right - p_cam_c)*(RIGHT_COLUMN_COORDS - p_world_c).transpose();
    std::cout<<"S:"<<S<<std::endl;

    //SVD分解
    Eigen::JacobiSVD<Eigen::Matrix2f> svd(S,Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix2f U = svd.matrixU();
    Eigen::Matrix2f V = svd.matrixV();
    Eigen::MatrixXf sigma = svd.singularValues();
    std::cout<<"U="<<U<<",V="<<V<<",sigma:"<<sigma<<std::endl;
    Eigen::Matrix2f A = Eigen::Matrix2f::Zero();
    A(0,0) = sigma(0,0);
    A(1,1) = sigma(1,0);
    Eigen::MatrixXf res = U*A*V;
    std::cout<<res<<std::endl;
    Eigen::MatrixXf VUT = V*U.transpose();
    Eigen::Matrix2f M = Eigen::Matrix2f::Identity();
    M(1,1) = VUT.determinant();
    // if (VUT.determinant() < -1e-5){M(1,1) = -1;}
    Eigen::MatrixXf R_w2c = V*M*U.transpose();
    Eigen::VectorXf chasis_coords = LEFT_COLUMN_COORDS - R_w2c*p_cam_left;
    std::cout<<"solve R:"<<R_w2c<<std::endl;
    std::cout<<"solve camera coords:"<<chasis_coords<<std::endl;
}